#include "Ch_Ctrl.h"

DT35_Handler_t dt35_1;
Comm_WindowData_t dt351_window;

#define Chassis_L 650.0f
#define Chassis_W 400.0f
extern int rocker_lx;
extern int rocker_ly;
uint8_t cycle = 0;

int16_t speed_x,speed_y, rotate_spd;

void ChCtrl_Init_DT35(void)
{
    DT35_Init(&dt35_1, 300.0f , 500.0f ,
              LASER_ADC_ONE, LASER_CHANNEL_THREE, LASER_GPIO_THREE,
              LASER_PIN_THREE); 
    DT35_Setup_KalmanFilter(&dt35_1, 1.0f, 0.0001, 0.05f, 0.0f);
    WindowFloat_Init_Slide(&dt351_window, 40);
}

volatile float dt_data[2];
volatile float window_var;

/**
* @name:ChCtrl_Test_DT35
* @brief:DT35测试
**/

void ChCtrl_Test_DT35(void)
{
    dt_data[0] = DT35_Get_Kalman(&dt35_1);

    JustFloat_Printf(dt_data, 1, USART1_MAP);   //按JustFloat格式向VOFA上位机输送波形数据

    WindowFloat_Update_Member(&dt351_window, dt_data[1]);  //将数据送入窗口中
    window_var = WindowFloat_Get_Var(&dt351_window);       //获取窗口数据的方差
}

/**
* @name:ChCtrl_Laser_Main
* @brief：返回DT35经卡尔曼滤波处理后得到的距离值
**/

volatile float mm_dt1;    //dt35通过卡尔曼滤波后得到的距离值

void ChCtrl_Laser_Main(void)
{
    mm_dt1 = DT35_Get_Kalman(&dt35_1);
}

/**
* @name:ChCtrl_DT35_Get_dist
* @brief：返回DT35经卡尔曼滤波处理后得到的距离值
**/

float ChCtrl_DT35_Get_dist(void)
{
    return DT35_Get_Kalman(&dt35_1);
}

/**
* @name:BaseMove_Joystick_Test
* @brief：手柄控制底盘移动测试函数
**/
void BaseMove_Joystick_Test(void)
{
	if(cycle == 0)
	{
		rocker_ly = 128;
		rocker_lx = 128;
		cycle = 1;
	}
	else
	{
		if(abs(rocker_ly - 128) < 10)
		{
			rocker_ly = 128;
		}
		if (abs(rocker_lx - 128) < 10) 
		{
			rocker_lx = 128;
		}
		if (abs(rocker_rx - 128) < 10)
		{
			rocker_rx = 128;
		}
	}
	speed_x = ((float)(rocker_lx - 128)) * 1.0;
	speed_y = ((float)(rocker_ly - 128)) * 1.0;
	rotate_spd = 0;
	AGV_Re_Center_Move_v3_3(speed_x, speed_y, rotate_spd);
}

